#include <ctime>
#include <functional>
#include <opencv2/opencv.hpp>

#include "interface/plan.h"

using Func = std::function<void(std::vector<std::vector<int>> &, std::vector<Point> &)>;

void runTests() {}

void CreateMap1(std::vector<std::vector<int>> &grid, std::vector<Point> &goals) {
    std::vector<std::vector<int>> _grid{{1, 1, 1, 1, 1, 1, 1, 1}, {2, 2, 2, 2, 2, 2, 2, 2}, {1, 1, 1, 1, 1, 1, 1, 1},
                                        {1, 1, 1, 1, 1, 1, 1, 1}, {2, 2, 2, 2, 2, 2, 2, 2}, {1, 1, 1, 1, 1, 1, 1, 1},
                                        {1, 1, 1, 8, 1, 1, 1, 1}, {2, 2, 2, 2, 2, 2, 2, 2}, {1, 1, 1, 1, 1, 1, 1, 1}};

    // 随机生成N个点的坐标
    std::vector<Point> _goals(72);
    for (int i = 0; i < _goals.size(); ++i) {
        int x = std::rand() % (_grid[0].size());
        int y = std::rand() % (_grid.size());
        // 如果是障碍物，跳过这个目标点
        if (_grid[y][x] == kObstacle)
            continue;

        _goals[i].x = x;
        _goals[i].y = y;
    }

    grid  = _grid;
    goals = _goals;
}

void CreateMap2(std::vector<std::vector<int>> &grid, std::vector<Point> &goals) {
    std::vector<std::vector<int>> _grid{{1, 1, 1, 1, 1, 1, 1, 1}, {2, 2, 2, 2, 2, 2, 2, 2}, {1, 1, 1, 1, 1, 1, 1, 1},
                                        {1, 1, 1, 1, 1, 1, 1, 1}, {2, 2, 2, 2, 2, 2, 2, 2}, {1, 1, 1, 1, 1, 1, 1, 1},
                                        {1, 1, 1, 8, 1, 1, 1, 1}, {2, 2, 2, 2, 2, 2, 2, 2}, {1, 1, 1, 1, 1, 1, 1, 1}};

    // 随机生成N个点的坐标
    std::vector<Point> _goals;
    for (int x = 0; x < _grid[0].size(); x++) {
        for (int y = 0; y < _grid.size(); y++) {
            // 如果是障碍物，跳过这个目标点
            if (_grid[y][x] == kObstacle)
                continue;
            _goals.emplace_back(Point(x, y));
        }
    }

    grid  = _grid;
    goals = _goals;
}

void CreateMap3(std::vector<std::vector<int>> &grid, std::vector<Point> &goals) {
    // 地图的宽和高
    int width  = 80;
    int height = 100;

    // 母道的行数集合
    std::vector<int>              cross_rows{0, 5, 15, 25, 35, 45, 55, 65, 80, 90};
    std::vector<std::vector<int>> _grid;

    // 生成地图，默认是子道
    _grid.resize(height, std::vector<int>(width, GridType::kNormal));

    // 填充地图
    for (int i = 0; i < cross_rows.size(); i++) {
        int row = cross_rows[i];
        // 填充母道
        for (int col = 0; col < width; col++) {
            _grid[row][col] = GridType::kCross;
        }
    }

    // 随机生成10个障碍物
    for (int i = 0; i < 10; i++) {
        int row         = rand() % height;
        int col         = rand() % width;
        _grid[row][col] = GridType::kObstacle;
    }

    // 随机生成N个点的坐标
    std::vector<Point> _goals(50);
    int                loop_cnt = 100;

    for (int i = 0; i < _goals.size(); ++i) {
        int x = std::rand() % (_grid[0].size());
        int y = std::rand() % (_grid.size());
        // 如果是障碍物，跳过这个目标点
        if (_grid[y][x] == kObstacle)
            continue;

        _goals[i].x = x;
        _goals[i].y = y;
    }

    grid  = _grid;
    goals = _goals;
}

int main() {

    // 以当前时间作为随机种子, 随机生成N个点的坐标
    std::srand(static_cast<unsigned int>(std::time(nullptr)));

    std::vector<Func> funcs;
    // 将生成地图的函数添加到funcs中
    funcs.emplace_back(CreateMap1);
    funcs.emplace_back(CreateMap2);
    funcs.emplace_back(CreateMap3);

    std::vector<std::vector<int>> grid;
    std::vector<Point>            goals;
    for (auto func : funcs) {
        func(grid, goals);
        PlanInterface    *planer = PlanInterface::create(grid);
        Point             start(0, 0);  // 初始点设置为(0,0）
        std::vector<Path> paths = planer->makePlan(start, goals);

        // opencv可视化
        cv::Mat image(grid.size(), grid[0].size(), CV_8UC3, cv::Scalar(128, 128, 128));
        // 填充目标点的颜色
        for (const auto &goal : goals) {
            image.at<cv::Vec3b>(goal.y, goal.x) = cv::Vec3b(0, 0, 255);
        }

        // 填充母道和子道颜色
        for (int row = 0; row < image.rows; row++) {
            for (int col = 0; col < image.cols; col++) {
                if (grid[row][col] == GridType::kCross) {  // 母道(白色)
                    image.at<cv::Vec3b>(row, col) = cv::Vec3b{255, 255, 255};
                }
                if (grid[row][col] == GridType::kObstacle) {  // 障碍物(黑色)
                    image.at<cv::Vec3b>(row, col) = cv::Vec3b{0, 0, 0};
                }
            }
        }

        cv::namedWindow("image", cv::WINDOW_NORMAL);
        cv::resizeWindow("image", 800, 600);
        cv::imshow("image", image);
        cv::waitKey(500);

        // 显示轨迹
        for (const auto &path : paths) {
            Point start                           = path.front();
            Point goal                            = path.back();
            image.at<cv::Vec3b>(start.y, start.x) = cv::Vec3b(255, 0, 0);
            image.at<cv::Vec3b>(goal.y, goal.x)   = cv::Vec3b(255, 0, 0);
            for (const auto p : path) {
                image.at<cv::Vec3b>(p.y, p.x) = cv::Vec3b(0, 255, 0);
                cv::namedWindow("image", cv::WINDOW_NORMAL);
                cv::resizeWindow("image", 800, 600);
                cv::imshow("image", image);
                cv::waitKey(50);
            }
        }

        // 数据统计
        int total_dist = 0;
        for (auto path : paths) {
            total_dist += path.size();
        }
        std::cout << "map:" << grid.size() << "x" << grid[0].size() << std::endl;
        std::cout << "start:" << start << std::endl;
        std::cout << "goals size:" << goals.size() << ", total distance:" << total_dist << std::endl;
    }

    return 0;
}